#pragma once

#include "bspline_opt_sep_impl.h"

namespace outline {

//////////////////////
// unseparated mode //
//////////////////////

template <typename R>
class bspline_opt_base<R>::uvariable : public variable_base {
public:
	///the method builds an equation for the variables' set:
	///point of a bezier piece from the left side (if left_piece) or from the right side,
	///corresponsing to the parameter value (t) should coinside with goal point;
	///the equations' pair is formulated as follows:
	///coefs0^T * vars = goal.x,
	///coefs1^T * vars = goal.y,
	///coefs should point on preallocated space of size() real cells;
	///d = 0 for x-coordinate, d = 1 for y-coordinate
	virtual void eq(uint d, bool left_piece, real t, real * coef) const = 0;
	///modifies right-hand-side of the least-square-error equation according 
	///to the inertia and attractor of the variables' set;
	///rhs points on filled space of size() real cells;
	///double type of rhs is a requirement of mtx library
	virtual void traction_modify(double * coef, double * rhs) const = 0;
};

///position of a bezier node point
template <typename R>
class bspline_opt_base<R>::ucenter : public virtual uvariable 
{ 
public:
	///given solution of equations, extracts position of the node from it
	///double type of s is a requirement of mtx library;
	virtual point get_position(double const * s) const = 0;
};

///right tangent of a bezier node
template <typename R>
class bspline_opt_base<R>::utangent : public virtual uvariable
{ 
public:
	///given solution of equations, extracts tangent of the node from it
	///double type of s is a requirement of mtx library;
	virtual point get_tangent(double const * s) const = 0;
};

///tangents of a bezier node
template <typename R>
class bspline_opt_base<R>::utangents : public virtual uvariable
{ 
public:
	///given solution of equations, extracts both tangents of the node from it
	///double type of s is a requirement of mtx library;
	virtual void get_tangents(double const * s, point & left, point & right) const = 0;
};

///all the data of a bezier node
template <typename R>
class bspline_opt_base<R>::unode : public virtual uvariable
{ 
public:
	///given solution of equations, extracts all the data of the node from it,
	///double type of s is a requirement of mtx library;
	virtual void get_data(double const * s, point & center, point & left, point & right) const = 0;
};

///variable a bezier node whose minimization alone may be separated on x & y passes
template <typename R>
template <class C, class B> //subclasses of variable and uvariable respectively
class bspline_opt_base<R>::sep_base : public B
{
public:
	C adaptee;
public:
	virtual uint size() const 
		{ return 2 * adaptee.size(); }
	virtual void goal_modify(bool left_piece, real t, point & goal) const
		{ return adaptee.goal_modify(left_piece, t, goal); }
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		uint s = adaptee.size();
		if (d == 0) {
			adaptee.eq(left_piece, t, coef);
			std::fill(coef + s, coef + 2*s, real(0));
		}
		else {
			std::fill(coef, coef + s, real(0));
			adaptee.eq(left_piece, t, coef + s);
		}
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		uint s = adaptee.size();
		adaptee.traction_modify(coef, rhs, rhs + s); 
		std::copy(coef, coef + s, coef + s);
	}
};

///center point of a bezier node whose minimization indeed may be separated on x & y passes
template <typename R>
template <class C> //subclass of center
class bspline_opt_base<R>::sep_center : public sep_base<C, ucenter> 
{
public:
	virtual point get_position(double const * s) const {
		return point(
			adaptee.get_position(0, s),
			adaptee.get_position(1, s + adaptee.size()));
	}
};

template <typename R>
template <class C> //subclass of tangent
class bspline_opt_base<R>::sep_tangent : public sep_base<C, utangent> 
{
public:
	virtual point get_tangent(double const * s) const {
		return point(
			adaptee.get_tangent(0, s),
			adaptee.get_tangent(1, s + adaptee.size()));
	}
};

template <typename R>
class bspline_opt_base<R>::fixed_utangent : public sep_tangent<fixed_tangent>
{
public:
	fixed_utangent() 
		{ }
	fixed_utangent(point const & tangent) 
		{ adaptee.set_tangent(tangent); }
};

template <typename R>
class bspline_opt_base<R>::fixed_direction_tangent : public utangent
{
public:
	virtual uint size() const 
		{ return 1; }
	virtual void goal_modify(bool left_piece, real t, point & goal) const
		{ /* nothing */ }
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		if (left_piece)
			coef[0] = 0;
		else
			coef[0] = 3 * t * (1 - t) * (1 - t) * direction_[d];
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		rhs[0] += coef[0] * pp_;
		coef[0] *= tt_;
	}
	virtual point get_tangent(double const * s) const 
		{ return real(s[0]) * direction_; }
public:
	fixed_direction_tangent() : pp_(0), tt_(1)
		{ }
	void set_direction(point const & d) {
		assert (d.x != 0 || d.y != 0);
		direction_ = d;
	}
	void set_traction(real attractor, real traction) {
		assert (traction >= 0 && traction < 1);
		//length_attractor = attractor;
		//length_traction = traction;
		tt_ = square(traction / (1 - traction));
		pp_ = attractor * tt_;
		tt_ += 1;
	}
private:
	point direction_;
	//real length_attractor;
	//real length_traction;
	real pp_, tt_;
};

///left and right tangents of a cusp bezier node
template <typename R>
class bspline_opt_base<R>::independent_utangents : public utangents {
public:
	virtual uint size() const
		{ return left_size_ + right_->size(); }
	virtual void goal_modify(bool left_piece, real t, point & goal) const {
		left_->goal_modify(!left_piece, 1 - t, goal);
		right_->goal_modify(left_piece, t, goal);
	}
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		left_->eq(d, !left_piece, 1 - t, coef);
		right_->eq(d, left_piece, t, coef + left_size_);
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		left_->traction_modify(coef, rhs);
		right_->traction_modify(coef + left_size_, rhs + left_size_);
	}
	virtual void get_tangents(double const * s, point & left, point & right) const {
		left = left_->get_tangent(s);
		right = right_->get_tangent(s + left_size_);
	}
public:
	independent_utangents() { }
	independent_utangents(utangent_sptr const & l, utangent_sptr const & r) 
		: left_(l), right_(r)
		{ left_size_ = left_->size(); }
	void set_left(utangent_sptr const & l) 
		{ left_ = l; left_size_ = left_->size(); }
	void set_right(utangent_sptr const & r) 
		{ right_ = r; }
private:
	utangent_sptr left_, right_;
	uint left_size_;
};

///left and right tangents of a flat bezier node
template <typename R>
class bspline_opt_base<R>::zero_utangents : public utangents {
public:
	virtual uint size() const
		{ return 0; }
	virtual void goal_modify(bool left_piece, real t, point & goal) const
		{ /* nothing */ }
	virtual void eq(uint d, bool left_piece, real t, real * coef) const
		{ /* nothing */ }
	virtual void traction_modify(double * coef, double * rhs) const
		{ /* nothing */ }
	virtual void get_tangents(double const * s, point & left, point & right) const 
		{ left = right = point(0, 0); }
};

///tangents of a symmetrical or smooth_affine bezier node
template <typename R>
class bspline_opt_base<R>::fixed_relength_utangents : public utangents {
public:
	virtual uint size() const
		{ return 2; }
	virtual void goal_modify(bool left_piece, real t, point & goal) const
		{ /* nothing */ }
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		coef[1-d] = 0;
		if (left_piece)
			coef[d] = l_ * 3 * t * t * (1 - t);
		else
			coef[d] = r_ * 3 * t * (1 - t) * (1 - t);
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		rhs[0] += coef[0] * pp_.x;
		rhs[1] += coef[1] * pp_.y;
		coef[0] *= tt_;
		coef[1] *= tt_;
	}
	virtual void get_tangents(double const * s, point & left, point & right) const {
		point d(s[0], s[1]);
		left = l_ * d;
		right = r_ * d;
	}
public:
	fixed_relength_utangents() : pp_(0, 0), tt_(1)
		{ }
	void set_ratio(real l, real r)
		{ l_ = l; r_ = r; assert (l != 0 || r != 0); }
	void set_traction(point const & attractor, real traction) {
		assert (traction >= 0 && traction < 1);
		//attractor_ = attractor;
		//traction_ = traction;
		tt_ = square(traction / (1 - traction));
		pp_ = attractor * tt_;
		tt_ += 1;
	}
private:
	real l_, r_; ///< relative lengths of tangents
	//point attractor_;
	//real traction_;
	point pp_;
	real tt_;
};

///tangents of a symmetrical or smooth_affine bezier node with fixed direction
template <typename R>
class bspline_opt_base<R>::fixed_direction_fixed_relength_utangents : public utangents {
public:
	virtual uint size() const
		{ return 1; }
	virtual void goal_modify(bool left_piece, real t, point & goal) const
		{ /* nothing */ }
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		if (left_piece)
			coef[0] = l_ * 3 * t * t * (1 - t) * direction_[d];
		else
			coef[0] = r_ * 3 * t * (1 - t) * (1 - t) * direction_[d];
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		rhs[0] += coef[0] * pp_;
		coef[0] *= tt_;
	}
	virtual void get_tangents(double const * s, point & left, point & right) const {
		left = l_ * real(s[0]) * direction_;
		right = r_ * real(s[0]) * direction_;
	}
public:
	fixed_direction_fixed_relength_utangents() : pp_(0), tt_(1)
		{ }
	void set_direction(point const & d) {
		assert (d.x != 0 || d.y != 0);
		direction_ = d;
	}
	void set_ratio(real l, real r)
		{ l_ = l; r_ = r; assert (l != 0 || r != 0); }
	void set_traction(real attractor, real traction) {
		assert (traction >= 0 && traction < 1);
		//length_attractor = attractor;
		//length_traction = traction;
		tt_ = square(traction / (1 - traction));
		pp_ = attractor * tt_;
		tt_ += 1;
	}
private:
	point direction_;
	real l_, r_; ///< relative lengths of tangents
	//real length_attractor;
	//real length_traction;
	real pp_, tt_;
};

template <typename R>
class bspline_opt_base<R>::composite_unode : public unode {
public:
	virtual uint size() const 
		{ return center_size_ + tangents_->size(); }
	virtual void goal_modify(bool left_piece, real t, point & goal) const {
		center_->goal_modify(left_piece, t, goal);
		tangents_->goal_modify(left_piece, t, goal);
	}
	virtual void eq(uint d, bool left_piece, real t, real * coef) const {
		center_->eq(d, left_piece, t, coef);
		tangents_->eq(d, left_piece, t, coef + center_size_);
	}
	virtual void traction_modify(double * coef, double * rhs) const {
		center_->traction_modify(coef, rhs);
		tangents_->traction_modify(coef + center_size_, rhs + center_size_);
	}
	virtual void get_data(double const * s, point & center, point & left, point & right) const {
		center = center_->get_position(s);
		tangents_->get_tangents(s + center_size_, left, right);
	}
public:
	void set_center(ucenter_sptr const & c) 
		{ center_ = c; center_size_ = center_->size(); }
	void set_tangents(utangents_sptr const & t) 
		{ tangents_ = t; }
private:
	uint center_size_;
	ucenter_sptr center_;
	utangents_sptr tangents_;
};

// uspline

///this class describes overdetermined set of equations for unseparated spline fitting
template <typename R>
class bspline_opt_base<R>::uspline {
public:
	///necessarily call update_smooth_nodes(...) after create(...)
	void create(bool closed, uint nsize,
		std::vector<junc_type> const & junc, controls<R> const & ctrls,
		point_vector const * attractors = 0, real_vector const * tractions = 0,
		std::vector<int> const * fixed_dir_tangents = 0);
	///loads new fixed directions in the solvers for smooth nodes
	void update_smooth_nodes(controls<R> const & ctrls,
		point_vector const * attractors = 0, real_vector const * tractions = 0);
	uint nsize() const
		{ return nodes_.size(); }
	uint vsize() const
		{ return b_.size(); }
public: //formulate and solve overdetermined equations
	void eq(uint piece, real t, point goal);
	void eq(uint piece, real t, point, point goal) { eq(piece, t, goal); }
	void eq_solve(controls<R> & ctrls);
private:
	void eq_reset();
	static void normalize(point & direction);
	static void update_fixed_direction(fixed_direction_tangent * t, uint lr, uint c,
		controls<R> const & ctrls, point_vector const * attractors, real_vector const * tractions);
private:
	typedef mtx::dynamic::vector vector;
	typedef mtx::dynamic::matrix matrix;
	struct fixed_dir_item {
		fixed_direction_tangent * t;
		uint c, lr; ///< center and control point indeces in the spline
		fixed_dir_item() { }
		fixed_dir_item(fixed_direction_tangent * tt, uint cc, uint lrlr) : t(tt), c(cc), lr(lrlr) { }
	};
private:
	bool closed_;
	unode_vector nodes_;
	uint_vector var_begin_; ///< var_begin_[i] contains start index of node[i] inside variables vector
	matrix m_;
	vector b_;
	std::vector<fixed_dir_item> fixed_dir_vector_; ///< list of all fixed_direction tangents
};

template <typename R>
void bspline_opt_base<R>::uspline::normalize(point & direction)
{
	real abs_d = abs(direction);
	if (abs_d > 0)
		direction *= 1 / abs_d;
	else
		direction = point(1, 0); //any direction
}

template <typename R>
void bspline_opt_base<R>::uspline::update_fixed_direction(fixed_direction_tangent * t, uint lr, uint c,
	controls<R> const & ctrls, point_vector const * attractors, real_vector const * tractions)
{
	point direction = ctrls[lr] - ctrls[c];
	normalize(direction);
	t->set_direction(direction);
	if (tractions)
		t->set_traction(
			scalar_product((*attractors)[lr] - (*attractors)[c], direction), (*tractions)[lr]);
}

template <typename R>
void bspline_opt_base<R>::uspline::create(bool closed, uint nsize,
	std::vector<junc_type> const & junc, controls<R> const & ctrls,
	point_vector const * attractors, real_vector const * tractions,
	std::vector<int> const * fixed_dir_tangents)
{
	closed_ = closed;
	uint s = nsize;
	assert (junc.size() >= s);
	if (attractors)
		assert (closed && attractors->size() >= 3*s && tractions->size() >= 3*s ||
				!closed && attractors->size() >= 3*s-2 && tractions->size() >= 3*s-2);
	if (fixed_dir_tangents)
		assert (fixed_dir_tangents->size() == s);
	nodes_.resize(s);
	var_begin_.resize(s+1);
	fixed_dir_vector_.clear();

	uint vsize = 0;
	var_begin_[0] = 0;
	for (uint i = 0; i < s; ++i) 
	{
		uint l = (i == 0) ? 3*s-1 : 3*i-1; //not valid for open splines and i == 0
		uint c = 3*i;
		uint r = 3*i+1; //not valid for open splines and i == s-1

		bool left_dir_fixed = fixed_dir_tangents && ((*fixed_dir_tangents)[i] & fixed_left_tan_dir) != 0;
		bool right_dir_fixed = fixed_dir_tangents && ((*fixed_dir_tangents)[i] & fixed_right_tan_dir) != 0;

		composite_unode * node = new composite_unode();
		nodes_[i] = unode_sptr(node);

		if (attractors && (*tractions)[c] >= 1) {
			fixed_ucenter * center = new fixed_ucenter();
			node->set_center(ucenter_sptr(center));
			center->adaptee.set_position((*attractors)[c]);
		}
		else {
			mobile_ucenter * center = new mobile_ucenter();
			node->set_center(ucenter_sptr(center));
			if (attractors) {
				center->adaptee.set_traction((*attractors)[c], (*tractions)[c]);
			}
		}

		switch (junc[i]) {
		case junc_flat:
			node->set_tangents(utangents_sptr(new zero_utangents()));
			break;
		case junc_symmetrical: 
		case junc_smooth_affine: 
		case junc_smooth: 
		{
			if (!closed_ && tractions) { 
				if (i == 0 && (*tractions)[r] >= 1) {
					node->set_tangents(utangents_sptr(new independent_utangents(
						utangent_sptr(new zero_utangent()),
						utangent_sptr(new fixed_utangent((*attractors)[r] - (*attractors)[c]))
					)));
					break;
				}
				if (i+1 == s && (*tractions)[l] >= 1) {
					node->set_tangents(utangents_sptr(new independent_utangents(
						utangent_sptr(new fixed_utangent((*attractors)[l] - (*attractors)[c])),
						utangent_sptr(new zero_utangent())
					)));
					break;
				}
			}
			assert (closed_ || i > 0 && i+1 < s);

			real mean_traction;
			if (tractions)
				mean_traction = real(0.5) * ((*tractions)[l] + (*tractions)[r]);

/*			if (junc[i] == junc_symmetrical && (!tractions || (*tractions)[l] < 1 || (*tractions)[r] < 1)) {
				if (!left_dir_fixed && !right_dir_fixed) {
					symmetrical_utangents * stangents = new symmetrical_utangents();
					node->set_tangents(utangents_sptr(stangents));
					if (tractions)
						stangents->set_traction(tangent_attractor_dir, mean_traction);
				}
				else {
					fixed_direction_symmetrical_utangents * stangents = new fixed_direction_symmetrical_utangents();
					node->set_tangents(utangents_sptr(stangents));
					point direction = ctrls[r] - ctrls[l];
					normalize(direction);
					stangents->set_direction(direction);
					if (tractions) {
						stangents->set_traction(
							scalar_product(direction, tangent_attractor_dir), mean_traction);
					}
				}
				break;
			}*/

			if ((junc[i] == junc_smooth_affine || junc[i] == junc_symmetrical) 
				&& (!tractions || (*tractions)[l] < 1 || (*tractions)[r] < 1)) 
			{
				real jl = -1, jr = 1;
				if (junc[i] != junc_symmetrical)
					ctrls.calc_tangents_relength(l, c, r, jl, jr, true);

				if (!left_dir_fixed && !right_dir_fixed) {
					fixed_relength_utangents * tangents = new fixed_relength_utangents();
					node->set_tangents(utangents_sptr(tangents));
					tangents->set_ratio(jl, jr);
					if (tractions) {
						if (fabs(jl) > fabs(jr))
							tangents->set_traction(((*attractors)[l] - (*attractors)[c]) / jl, mean_traction);
						else
							tangents->set_traction(((*attractors)[r] - (*attractors)[c]) / jr, mean_traction);
					}
				}
				else {
					fixed_direction_fixed_relength_utangents * tangents = new fixed_direction_fixed_relength_utangents();
					node->set_tangents(utangents_sptr(tangents));
					tangents->set_ratio(jl, jr);
					point direction = ctrls[r] - ctrls[l];
					normalize(direction);
					tangents->set_direction(direction);
					if (tractions) {
						if (fabs(jl) > fabs(jr))
							tangents->set_traction(
								scalar_product(direction, ((*attractors)[l] - (*attractors)[c])) / jl, mean_traction);
						else
							tangents->set_traction(
								scalar_product(direction, ((*attractors)[r] - (*attractors)[c])) / jr, mean_traction);
					}
				}
				break;
			}

			independent_utangents * itangents = new independent_utangents();
			node->set_tangents(utangents_sptr(itangents));

			if (tractions && (*tractions)[l] >= 1)
				itangents->set_left(utangent_sptr(new fixed_utangent((*attractors)[l] - (*attractors)[c])));
			else {
				fixed_direction_tangent * left = new fixed_direction_tangent();
				itangents->set_left(utangent_sptr(left));
				if (left_dir_fixed)
					update_fixed_direction(left, l, c, ctrls, attractors, tractions);
				else
					fixed_dir_vector_.push_back(fixed_dir_item(left, c, l));
			}

			if (tractions && (*tractions)[r] >= 1)
				itangents->set_right(utangent_sptr(new fixed_utangent((*attractors)[r] - (*attractors)[c])));
			else {
				fixed_direction_tangent * right = new fixed_direction_tangent();
				itangents->set_right(utangent_sptr(right));
				if (right_dir_fixed)
					update_fixed_direction(right, r, c, ctrls, attractors, tractions);
				else
					fixed_dir_vector_.push_back(fixed_dir_item(right, c, r));
			}
			break;
		}
		case junc_cusp: {
			independent_utangents * tangents = new independent_utangents();
			node->set_tangents(utangents_sptr(tangents));
			if (!closed_ && i == 0)
				tangents->set_left(utangent_sptr(new zero_utangent()));
			else {
				if (attractors && (*tractions)[l] >= 1)
					tangents->set_left(utangent_sptr(new fixed_utangent((*attractors)[l] - (*attractors)[c])));
				else if (left_dir_fixed) {
					fixed_direction_tangent * left = new fixed_direction_tangent();
					tangents->set_left(utangent_sptr(left));
					update_fixed_direction(left, l, c, ctrls, attractors, tractions);
				}
				else {
					arbitrary_utangent * left = new arbitrary_utangent();
					tangents->set_left(utangent_sptr(left));
					if (attractors)
						left->adaptee.set_traction((*attractors)[l] - (*attractors)[c], (*tractions)[l]);
				}
			}
			if (!closed_ && i == s-1)
				tangents->set_right(utangent_sptr(new zero_utangent()));
			else {
				if (attractors && (*tractions)[r] >= 1)
					tangents->set_right(utangent_sptr(new fixed_utangent((*attractors)[r] - (*attractors)[c])));
				else if (right_dir_fixed) {
					fixed_direction_tangent * right = new fixed_direction_tangent();
					tangents->set_right(utangent_sptr(right));
					update_fixed_direction(right, r, c, ctrls, attractors, tractions);
				}
				else {
					arbitrary_utangent * right = new arbitrary_utangent();
					tangents->set_right(utangent_sptr(right));
					if (attractors)
						right->adaptee.set_traction((*attractors)[r] - (*attractors)[c], (*tractions)[r]);
				}
			}
			break;
		}
		default:
			assert (false);
		}

		vsize += nodes_[i]->size();
		var_begin_[i+1] = vsize;
	}
	//assert (!fixed_dir_vector_.empty()); //otherwise you better to use separated solver

	m_.resize(vsize, vsize);
	b_.resize(vsize);
	eq_reset();
}

template <typename R>
inline void bspline_opt_base<R>::uspline::eq_reset()
{
	m_ = 0;
	b_ = 0;
}

template <typename R>
void bspline_opt_base<R>::uspline::eq(uint piece, real t, point goal)
{
	assert (piece >= 0);
	assert (closed_ && piece < nsize() || !closed_ && piece+1 < nsize());

	if (piece+1 == nsize()) {
		//last piece in closed uspline

		if (piece == 0) {
			//last and the only piece
			uint vs = vsize();
			unode & node0 = *nodes_[0];
			assert (node0.size() == vsize());
			real_vector coef0(vs);
			real_vector coef1(vs);

			node0.goal_modify(false, t, goal);
			node0.goal_modify(true, t, goal);

			for (uint d = 0; d < 2; ++d) {
				node0.eq(d, false, t, &coef0[0]);
				node0.eq(d, true, t, &coef1[0]);
				for (uint i = 0; i < vs; ++i)
					coef0[i] += coef1[i];

				for (uint i = 0; i < vs; ++i) {
					b_(i) += coef0[i] * goal[d];
					for (uint j = i; j < vs; ++j) {
						m_(i, j) += coef0[i] * coef0[j];
					}
				}
			}

			return;
		}

		unode & node0 = *nodes_[piece];
		unode & node1 = *nodes_[0];
		uint vb = var_begin_[piece];
		uint vm = var_begin_[piece+1];
		uint ve = var_begin_[1];
		assert (node0.size() == vm - vb);
		assert (node1.size() == ve);
		real_vector coef0(vm - vb);
		real_vector coef1(ve);

		node0.goal_modify(false, t, goal);
		node1.goal_modify(true, t, goal);

		for (uint d = 0; d < 2; ++d) {
			node0.eq(d, false, t, &coef0[0]);
			node1.eq(d, true, t, &coef1[0]);

			//fill only upper-right part of m_
			for (uint i = 0; i < coef0.size(); ++i) {
				b_(vb + i) += coef0[i] * goal[d];
				for (uint j = i; j < coef0.size(); ++j) {
					m_(vb + i, vb + j) += coef0[i] * coef0[j];
				}
			}
			for (uint i = 0; i < coef1.size(); ++i) {
				b_(i) += coef1[i] * goal[d];
				for (uint j = i; j < coef1.size(); ++j) {
					m_(i, j) += coef1[i] * coef1[j];
				}
			}
			for (uint i = 0; i < coef1.size(); ++i) {
				for (uint j = 0; j < coef0.size(); ++j) {
					assert (vb + j > i);
					m_(i, vb + j) += coef1[i] * coef0[j];
				}
			}
		}
		return;
	}

	unode & node0 = *nodes_[piece];
	unode & node1 = *nodes_[piece+1];
	uint vb = var_begin_[piece];
	uint vm = var_begin_[piece+1];
	uint ve = var_begin_[piece+2];
	assert (node0.size() == vm - vb);
	assert (node1.size() == ve - vm);
	real_vector coef(ve - vb);

	node0.goal_modify(false, t, goal);
	node1.goal_modify(true, t, goal);

	for (uint d = 0; d < 2; ++d) {
		node0.eq(d, false, t, &coef[0]);
		node1.eq(d, true, t, &coef[vm - vb]);

		//fill only upper-right part of m_
		for (uint i = 0; i < coef.size(); ++i) {
			b_(vb + i) += coef[i] * goal[d];
			for (uint j = i; j < coef.size(); ++j) {
				m_(vb + i, vb + j) += coef[i] * coef[j];
			}
		}
	}
}

template <typename R>
void bspline_opt_base<R>::uspline::eq_solve(controls<R> & ctrls)
{
	assert (closed_ && ctrls.pieces() == nsize() || !closed_ && ctrls.pieces()+1 == nsize());

	uint vsize = this->vsize();
	uint nsize = this->nsize();
	//make m_ symmetric by copying upper-right part into lower-left part
	for (uint i = 0; i < vsize; ++i)
		for (uint j = 0; j < i; ++j)
			m_(i, j) = m_(j, i);

	//modify equations according to the tractions
	std::vector<double> diag(vsize);
	for (uint i = 0; i < vsize; ++i)
		diag[i] = m_(i, i);
	for (uint i = 0; i < nsize; ++i) {
		uint vb = var_begin_[i];
		nodes_[i]->traction_modify(&diag[vb], &b_(0) + vb);
	}
	for (uint i = 0; i < vsize; ++i)
		m_(i, i) = diag[i];

	//solve equations
	//std::clog << "m:\n" << m_;
	//std::clog << "b:\n" << b_;
	uint_vector pivot(vsize-1);
	LU(m_, pivot);
	sle(m_, b_, pivot);
	//std::clog << "m-lup:\n" << m_;
	//std::clog << "b-sol:\n" << b_;

	//copy result into ctrls
	for (uint i = 0; i < nsize; ++i) 
	{
		uint l = (i == 0) ? 3*nsize-1 : 3*i-1; //not valid for open splines and i == 0
		uint c = 3*i;
		uint r = 3*i+1; //not valid for open splines and i == nsize-1

		uint vb = var_begin_[i];
		point center, left_tan, right_tan;
		nodes_[i]->get_data(&b_(0) + vb, center, left_tan, right_tan);
		if (closed_ || i > 0)
			ctrls[l] = center + left_tan;
		ctrls[c] = center;
		if (closed_ || i+1 < nsize)
			ctrls[r] = center + right_tan;
	}
	if (closed_)
		ctrls[3*nsize] = ctrls[0];
	eq_reset();
}

template <typename R>
void bspline_opt_base<R>::uspline::update_smooth_nodes(controls<R> const & ctrls,
	point_vector const * attractors, real_vector const * tractions)
{
	for (uint i = 0; i < fixed_dir_vector_.size(); ++i) {
		fixed_dir_item const & f = fixed_dir_vector_[i];
		update_fixed_direction(f.t, f.lr, f.c, ctrls, attractors, tractions);
	}
}

} //namespace outline
